#pragma once
#include <cmath>
#include <chrono>
#include <cstring>
#include <ctime>
#include <iostream>

// 三维空间向量结构体，用于表示三维空间中的点或方向向量
struct Vector3d
{
    double x, y, z;
    Vector3d(double x = 0, double y = 0, double z = 0) : x(x), y(y), z(z) {}
};

// 用于表示三维空间中的旋转变换矩阵
struct Matrix3x3
{
    double data[3][3];
    Matrix3x3(double matrix[3][3]);
    /**
     * @brief   矩阵与向量相乘
     * @param v 输入向量
     * @return  变换后的新向量
     */
    Vector3d multiply(const Vector3d &v) const;

    Matrix3x3 transpose() const;
};

// 封装所有数据，用于qt界面显示
struct AllData
{
    double car1_longitude;
    double car1_latitude;
    double car1_heading;
    double car1_deviation;
    double car2_longitude;
    double car2_latitude;
    double car2_heading;
    double car2_deviation;
    double drone_longitude;
    double drone_latitude;
    double drone_altitude;
    double drone_heading;
    double drone_roll;
    double drone_pitch;
    double drone_targetX;
    double drone_targetY;
    uint64_t t1;
    uint64_t t2;
    uint64_t t3;
    AllData();

    // 时间戳更新函数组
    void updateT1();

    void updateT2();

    void updateT3();

    // 获取当前时间戳（微秒级）
    static uint64_t getCurrentTimestamp();
    // 判断两个时间戳是否相差0.5秒内
    static bool isOk(uint64_t ts1, uint64_t ts2, uint64_t threshold_ = 100000);
};

// 角度转弧度
constexpr double degreesToRadians(double degrees) {
    return degrees * M_PI / 180.0;
}

// 将弧度转换为角度
constexpr double radiansToDegrees(double radians) {
    return radians * 180.0 / M_PI;
}
